	; System stack will be all addresses below 0x200
	.orig   0x200
SysStackTop:
	; This is our interrupt/exception handler
	.orig	0x200
	; Test if this is from our illegal-inst test
	; or from a real interrupt (test if rD is 13)
	subi	rD,rd,13
	bnez	rD,NotT1
	; Illegal instruction test, see if return address correct
	addi	a0,rd,0x0A02
	sw	a0,0xf800(rd)
	addi	a1,rc,0
	subi	rd,rc,Test1
	bnez	rd,Error
	;; See if cause (CTL1) value is correct
	xor	rd,rd,rd
	addi	a0,a0,0x0A03
	sw	a0,0xf800(rd)
	rctl	a1,ctl1
	bnez	a1,Error
	;; See if CTl0 value is correct
	;; It should have CM at 1, but OM, IE and OIE at 0
	addi	a0,rd,0x0A04
	sw	a0,0xf800(rd)
	rctl	a1,ctl0
	xori	rd,a1,0x4
	bnez	rd,Error
	;; Jump back, skipping the illegal inst and the 4 insts that follow it
	addi	ra,rc,20
	jrl	ra,0(ra)

TestD:
	.word 0xFFFFFFFF

NotT1:
	addi	re,re,1
	; Put rd back to what it should be (the test above subtracted 13)
	addi	rd,rd,13
	; Save a0,a1,a2 to system stack
	addi    rd,rd,-32
	sw	t0,0(rd)
	sw	t1,4(rd)
	sw	t2,8(rd)
	sw	t3,12(rd)
	sw	a0,16(rd)
	sw	a1,20(rd)
	sw	a2,24(rd)
	sw	a3,28(rd)
	
	xor	t0,t0,t0
	; Check nesting, should never be anything but 1
	addi	a0,t0,0x0A05
	or	a1,re,re
	subi	t1,a1,1
	bnez	t1,Error
	

	;  Check cause, should not be 0
	addi	a0,t0,0x0A06
	rctl	a1,ctl1
	beqz	a1,Error

	eqi	t1,a1,1
	beqz	t1,NoTimer
	
	; We had a timer interrupt, handle it
	; TCTL should have IP==1 
	addi	a0,t0,0x0A07
	lw	a1,0xf100(t0)
	andi	t1,a1,2
	beqz	t1,Error
	; Check CTL0, should be OM==1, CM==1, OIE==1, IE==0
	addi	a0,t0,0x0A08
	rctl	a1,ctl0
	or	a2,ir,ir
	xori	a3,a1,0xE
	bnez	a3,Error

	; Clear the IP bit in TCTL
	lw	a1,0xf100(t0)
	andi	a2,a1,0x2
	xor	a1,a1,a2
	sw	a1,0xf100(t0)
	; Increment TestD location
	lw	a1,TestD(t0)
	addi	a1,a1,1
	sw	a1,TestD(t0)
	;  Also display the TestD value in LEDR
	sw	a1,0xf804(t0)
	beq	t0,t0,DoneDev
NoTimer:
	eqi	t1,a1,2
	beqz	t1,NotKeys

	; We had a keys interrupt, handle it
	; KCTL should have IP==1
	addi	a0,t0,0x0A09
	lw	a1,0xf000(t0)
	andi	t1,a1,0x10
	beqz	t1,Error
	; Check CTL0, should be OM==0, CM==1, OIE==1, IE==0
	addi	a0,t0,0x0A0A
	rctl	a1,ctl0
	or	a2,ir,ir
	xori	a3,a1,0x6
	bnez	a3,Error
	; Clear the IP bit in KCTL
	lw	a1,0xf000(t0)
	andi	a2,a1,0x10
	xor	a1,a1,a2
	sw	a1,0xf000(t0)
	; Increment TestD location
	lw	a1,TestD(t0)
	addi	a1,a1,1
	sw	a1,TestD(t0)
	;  Also display the TestD value in LEDG
	sw	a1,0xf808(t0)
	beq	t0,t0,DoneDev
	
NotKeys:	
	eqi	t1,a1,3
	beqz	t1,NotSws
	
	; We had a switches interrupt, handle it
	; SCTL should have IP==1
	addi	a0,t0,0x0A0B
	lw	a1,0xf004(t0)
	andi	t1,a1,0x400
	beqz	t1,Error
	; Check CTL0, should be OM==0, CM==1, OIE==1, IE==0
	addi	a0,t0,0x0A0C
	rctl	a1,ctl0
	or	a2,ir,ir
	xori	a3,a1,0x6
	bnez	a3,Error
	; Clear the IP bit in SCTL
	lw	a1,0xf004(t0)
	andi	a2,a1,0x400
	xor	a1,a1,a2
	sw	a1,0xf004(t0)
	; Decrement TestD location
	lw	a1,TestD(t0)
	addi	a1,a1,-1
	sw	a1,TestD(t0)
	;  Also display the TestD value in LEDG
	sw	a1,0xf808(t0)
	beq	t0,t0,DoneDev

	
NotSws:
	; Had interrupt, but from none of the devices we have!
	addi	a0,t0,0x0A0D
	beq	t0,t0,Error

DoneDev:
	; Restore register values and return from interrupt
	lw	t0,0(rd)
	lw	t1,4(rd)
	lw	t2,8(rd)
	lw	t3,12(rd)
	lw	a0,16(rd)
	lw	a1,20(rd)
	lw	a2,24(rd)
	lw	a3,28(rd)
	addi	rd,rd,32
	addi	re,re,-1
	reti
	
	.orig	0x400
	
	; System starts up in system mode, check if CTL0 is correct
	xor	r0,r0,r0
	; We will keep the interrupt nesting level in RE
	or	re,r0,r0
	; Start testing!
	addi	a1,r0,0xAABBCCDD
	addi	a0,r0,0x0501
	sw	a0,0xf800(r0)
	rctl	a1,ctl0
	; CTL0 should initially be 0100 - old mode 0, current mode 1 (sys), old IE 0, current IE 0
	addi	r1,r0,0x4
	bne	a1,r1,Error
	;  Test if wctl-rctl works
	addi	a0,r0,0x0502
	sw	a0,0xf800(r0)
	ori	a1,a1,0xF
	wctl	ctl0,a1
	rctl	a1,ctl0
	xori	t1,a1,0xF
	bnez	t1,Error

	xor	r0,r0,r0
	;; Set RD to 13 (so handler can know we are here)
	addi	rd,r0,13
	; Transition to user mode with interrupts disabled
	wctl	ctl0,r0
	; rctl is now an illegal instruction, and
	; it should cause an exception even when IE is 0
Test1:
	rctl	r0,ctl0
	addi	a0,r0,0x0A01
	sw	a0,0xf800(r0)
	or	a1,r0,r0
	beq	a1,a1,Error

	; When we're back here we are in system mode again
	; (because the handler didn't use RETI)

	; Set system stack pointer
	xor	s0,s0,s0
	addi	rd,s0,SysStackTop
	;  Check if timer's counter (TCNT) is 0 now
	addi	a0,s0,0x0503
	sw	a0,0xf800(s0)
	lw	a1,0xF104(s0)
	bnez	a1,Error
	; Check if timer's control (TCTL) is 0 now
	addi	a0,s0,0x0504
	sw	a0,0xf800(s0)
	lw	a1,0xF100(s0)
	bnez	a1,Error
	; In TCTL, set IP to 1, but IE, AR, OR to 0
	ori	t0,s0,0x2
	sw	t0,0xF100(s0)
	; Check if TCTL got the new value
	addi	a0,s0,0x0505
	sw	a0,0xf800(s0)
	lw	a1,0xF100(s0)
	bne	a1,t0,Error
	; Enable interrupts in CTL0
	ori	t0,s0,0x5
	wctl	ctl0,t0
	;  Wait for a short while
	nop
	nop
	nop
	nop
	nop
	; Did we have an interrupt?
	; (We shouldn't, IE in TCTL is 0)
	addi	a0,s0,0x0506
	sw	a0,0xf800(s0)
	lw	a1,TestD(s0)
	addi	t0,a1,1
	bnez	t0,Error
	; Set IP and IE in TCTL
	ori	t0,s0,0x3
	sw	t0,0xF100(s0)
	; Wait for a short while again
	nop
	nop
	nop
	nop
	nop
	; Did we have an interrupt?
	; (We should have had one)
	addi	a0,s0,0x0507
	sw	a0,0xf800(s0)
	lw	a1,TestD(s0)
	bnez	a1,Error
	; Was the IP bit in TCTL cleared and IE left at 1?
	addi	a0,s0,0x0508
	sw	a0,0xf800(s0)
	lw	a1,0xF100(s0)
	xori	t0,a1,0x1
	bnez	t0,Error
	; Do we have the correct CTL0?
	addi	a0,s0,0x0509
	sw	a0,0xf800(s0)
	rctl	a1,ctl0
	xori	t0,a1,0x5
	bnez	t0,Error

	; Disable interrupts in CTL0
	ori	t0,s0,0x4
	wctl	ctl0,t0
	; Display CCC0 so we know it's still counting
	addi	a0,s0,0xCCC0
	sw	a0,0xF800(s0)
	; Start the counter for a 2-second (2000 miliseconds) count
	ori	t0,s0,2000
	sw	t0,0xF104(s0)
	; Now wait until the counter reaches zero, testing all the while
	; it it is strictly descending in value
WaitT:
	lw	t3,0xF100(s0)
	lw	t1,0xF104(s0)
	beqz	t1,DoneT
	le	t2,t1,t0
	or	t0,t1,t1
	bnez	t2,Skip601
	addi	a0,s0,0x601
	sw	a0,0xf800(s0)
	or	a1,t1,t1
	beq	s0,s0,Error
Skip601:
	andi	t2,t3,0xE
	beqz	t2,Skip602
	addi	a0,s0,0x602
	sw	a0,0xf800(s0)
	or	a1,t3,t3
	beq	s0,s0,Error	
Skip602:	
	beq	s0,s0,WaitT
DoneT:
	; Did we have an interrupt?
	; (We shouldn't, IE in CTL0 is 0)
	addi	a0,s0,0x0603
	sw	a0,0xf800(s0)
	lw	a1,TestD(s0)
	bnez	a1,Error
		
	; Did the counter set its IP bit?
	addi	a0,s0,0x0604
	sw	a0,0xf800(s0)
	lw	a1,0xF100(s0)
	xori	t0,a1,0x3
	bnez	t0,Error

	; Now set up counter for auto repeat, clear IP
	ori	t0,s0,0x5
	sw	t0,0xF100(s0)
	; Enable interrupts in CTL0
	rctl	t0,ctl0
	ori	t0,t0,0x1
	wctl	ctl0,t0

	; We will do this for five seconds
	addi	t1,s0,5
	; Start the counter for a 1-second (1000 miliseconds) count
	ori	t0,s0,1000
	sw	t0,0xF104(s0)
WaitC2:
	; Interrupt handler should increment TestD every second
	lw	t0,TestD(s0)
	;  Display CCC1,CCC2,etc.
	addi	a0,t0,0xCCC1
	sw	a0,0xF800(s0)
	bne	t0,t1,WaitC2

	; Stop interrupts from the counter
	sw	s0,0xF100(s0)
	; Reset TestD
	sw	s0,TestD(s0)
	; Enable interrupts from keys and clear IP and OR
	addi	t0,s0,0x20
	sw	t0,0xf000(s0)
	; Test if we KCTL got the new value for IE,IP,OR
	addi	a0,s0,0x701
	lw	a1,0xf000(s0)
	andi	t1,a1,0xFFF0
	bne	t1,t0,Error
	
	; Switch to user mode with interrupts enabled
	addi	t0,s0,0x1
	wctl	ctl0,t0
	
	xor	t0,t0,t0
Keys:
	lw	t1,0xf000(t0)	;  Gets 000F when no KEY is pushed, 0000 when all are
	andi	t1,t1,0xF
	xori	t1,t1,0xF	;  Flip KEY bits to make it easier to understand
	sw	t1,0xf800(t0)	;  Display KEY status
	addi	t2,t0,0xF
	bne	t1,t2,Keys	;  Keep displaying until all KEY are pushed together
	addi	t1,t0,1
	sw	t1,0xf800(t0)	;  Display 0001
	; Test if iterrupts have occurred, reset TestD
	addi	a0,t0,0x702
	lw	a1,TestD(t0)
	beqz	a1,Error
	sw	t0,TestD(t0)
NoKeys:
	lw	t1,0xf000(t0)
	andi	t1,t1,0xF
	xori	t1,t1,0xF
	bne	t1,t0,NoKeys	;  Wait until all KEY released
	; Test if iterrupts have occurred, reset TestD
	addi	a0,t0,0x703
	lw	a1,TestD(t0)
	beqz	a1,Error
	sw	t0,TestD(t0)
	
	addi	t1,t0,2
	sw	t1,0xf800(t0)	;  Display 0002, wait for KEY[0] to be pressed
PosKey:
	lw	t2,0xf000(t0)
	andi	t2,t2,0x1
	xori	t2,t2,1
	beqz	t2,PosKey
	; Test if exactly one iterrupt has occurred, reset TestD
	addi	a0,t0,0x704
	lw	a1,TestD(t0)
	subi	a2,a1,1
	bnez	a2,Error
	sw	t0,TestD(t0)
	
	addi	t1,t1,1
	sw	t1,0xf800(t0)	;  Display 0003, wait until KEY[0] released
	addi	t3,t0,1
NegKey:
	lw	t2,0xf000(t0)
	and	t2,t2,t3
	xor	t2,t2,t3
	beq	t2,t3,NegKey
	; Test if exactly one iterrupt has occurred
	addi	a0,t0,0x705
	lw	a1,TestD(t0)
	subi	a2,a1,1
	bnez	a2,Error

	; Display 5555 to signal switches test
	addi	t1,t0,0x5555
	sw	t1,0xf800(t0)
	; Wait for all switches to be in the off position
	; To help with this, switch state is displayed on LEDR
ClrSw:
	lw	t1,0xf004(t0)
	andi	t1,t1,0x3FF
	sw	t1,0xf804(t0)
	bnez	t1,ClrSw
	; Set TestD to 2 and display it
	addi	t1,t0,2
	sw	t1,TestD(t0)
	sw	t1,0xf800(t0)

	; Wait for a change in switches
WaitS0:
	lw	t1,0xf004(t0)
	andi	t1,t1,0x3FF
	beqz	t1,WaitS0
	; Check if SW[0] and IP are 1, IE is 0
	addi	a0,t0,0x705
	lw	a1,0xf004(t0)
	andi	t1,a1,0xfff
	xori	t1,t1,0x401
	bnez	t1,Error
	; Check if TestD is still 2 (SCTL has IE at 0)
	addi	a0,t0,0x706
	lw	a1,TestD(t0)
	subi	t2,a1,2
	bnez	t2,Error
	; Clear IP and OR, set IE in SCTL
	addi	t1,t0,0x0800
	sw	t1,0xf004(t0)
	; Check if SCTL is OK (and display it)
	addi	a0,t0,0x707
	lw	a1,0xf004(t0)
	sw	a1,0xf800(t0)
	xori	t1,a1,0x801
	bnez	t1,Error
	
	; Wait for SW[0] to be off again
WaitS1:	
	lw	t1,0xf004(t0)
	andi	t1,t1,0x3FF
	bnez	t1,WaitS1
	; Check if SW[0], OR, IP are 0, IE is 1
	addi	a0,t0,0x708
	lw	a1,0xf004(t0)
	andi	t1,a1,0x1fff
	xori	t1,t1,0x0800
	bnez	t1,Error
	; Check if TestD is now 1
	addi	a0,t0,0x709
	lw	a1,TestD(t0)
	subi	t2,a1,1
	bnez	t2,Error
	
	addi	s0,t0,4		;  Test 4
	lw	t2,ffour(t0)
	andi	t3,t2,0xff
	beq	s0,t3,SkipE4	;  If 8 lowermost bits are 4, it's OK
	addi	a0,s0,0		;  Error number to display
	addi	a1,t3,0		;  Value we got (should be 0004)
	beq	a0,a0,Error	;  Display error
SkipE4:
	addi	s0,s0,1		;  Test 5
	jrl	rv,TestJRL(t0)	;  Jump, RV should become equal to RetLbl
RetLbl:
	addi	a0,s0,0		; Didn't jump, error 5
	beq	a0,a0,Error	; Display error
TestJRL:
	addi	s0,s0,1		;  Test 6
	addi	t1,t0,RetLbl
	addi	a0,s0,0		; Error code
	addi	a1,rv,0		; Vale of RV
	bne	rv,t1,Error	; Display error if RV is wrong
	addi	s0,s0,1		; Test 7
	addi	t2,t0,TestJRL
	sub	t3,t2,rv	;  TestJRL-RetLbl should be 8
	addi	t1,t0,8
	addi	a0,s0,0		; Error code
	addi	a1,t3,0		; Value of difference
	bne	t3,t1,Error	; Display error difference not 8

	addi	s0,s0,1		; Test 8
	addi	t1,t0,8
	subi	t1,t1,10	; Should now be -2
	addi	t2,r0,-2
	addi	a0,s0,0		;  Error code
	addi	a1,t1,0		;  Value of RV
	bne	t1,t2,Error	;  Display error if RV is wrong
	
	addi	t1,t0,17
	addi	t2,t0,100
	addi	t3,t0,117

	addi	s0,s0,1		; Test 9
	addi	a0,s0,0		; Error code
	add	a1,t1,t2	; Should be 117
	bne	a1,t3,Error	; Display error is not

	addi	s0,s0,1		; Test A
	addi	a0,s0,0		; Error code
	sub	a1,t3,t1	; Should be 100
	bne	a1,t2,Error	; Display error if not

	addi	s0,s0,1		; Test B
	addi	a0,s0,0		; Error code
	lt	a1,t1,t2	; Should be 1
	subi	a2,a1,1
	bnez	a2,Error	; Display error if not

	addi	s0,s0,1		; Test C
	addi	a0,s0,0		; Error code
	lt	a1,t2,t1	; Should be 0
	bnez	a1,Error	; Display error if not

	addi	s0,s0,1		; Test D
	addi	a0,s0,0		; Error code
	lt	a1,t2,t2	; Should be 0
	bnez	a1,Error	; Display error if not
	 
	addi	s0,s0,1		; Test E
	addi	a0,s0,0		; Error code
	le	a1,t1,t2	; Should be 1
	subi	a2,a1,1
	bnez	a2,Error	; Display error if not

	addi	s0,s0,1		; Test F
	addi	a0,s0,0		; Error code
	le	a1,t2,t1	; Should be 0
	bnez	a1,Error	; Display error if not

	addi	s0,s0,1		; Test 0x10
	addi	a0,s0,0		; Error code
	le	a1,t2,t2	; Should be 1
	beqz	a1,Error	; Display error if not
	 
	addi	t1,t0,-17
	addi	t2,t0,100
	addi	t3,t0,117

	addi	s0,s0,1		; Test 0x11
	addi	a0,s0,0		; Error code
	add	a1,t3,t1	; Should be 100
	bne	a1,t2,Error	; Display error is not

	addi	s0,s0,1		; Test 0x12
	addi	a0,s0,0		; Error code
	sub	a1,t2,t1	; Should be 117
	bne	a1,t3,Error	; Display error if not

	addi	s0,s0,1		; Test 0x13
	addi	a0,s0,0		; Error code
	lt	a1,t1,t2	; Should be 1
	subi	a2,a1,1
	bnez	a2,Error	; Display error if not

	addi	s0,s0,1		; Test 0x14
	addi	a0,s0,0		; Error code
	lt	a1,t2,t1	; Should be 0
	bnez	a1,Error	; Display error if not

	addi	s0,s0,1		; Test 0x15
	addi	a0,s0,0		; Error code
	lt	a1,t1,t1	; Should be 0
	bnez	a1,Error	; Display error if not

Done:
	addi	a0,s0,0xD000
DLoop:	
	sw	a0,0xf800(t0)
	beq	a0,a0,DLoop
Error:	
	xor	t0,t0,t0
	ADDI	A0,A0,0xE000
ELoop:
	lw	t1,0xf000(t0)
	andi	t2,t1,0x1
	beqz	t2,ErrVal
	andi	t2,t1,0x2
	beqz	t2,SupVal
	SW	A0,0xf800(T0)
	BEQ	t0,t0,ELoop
ErrVal:
	SW	A1,0xf800(T0)
	BEQ	t0,t0,ELoop
SupVal:	
	SW	A2,0xf800(T0)
	BEQ	t0,t0,ELoop
ffour:	
	.word	0xffff0004
neg1:
	.word	-1
